# from launch import LaunchDescription
# from launch.actions import DeclareLaunchArgument
# from launch.substitutions import LaunchConfiguration
# from launch_ros.actions import Node
# from ament_index_python.packages import get_package_share_directory

# import os

# def generate_launch_description():
#     # 获取包路径
#     pkg_share = get_package_share_directory('ball_collector')
    
#     # 配置参数文件
#     config_dir = os.path.join(pkg_share, 'config')
#     detection_params = os.path.join(config_dir, 'detection_params.yaml')
#     robot_params = os.path.join(config_dir, 'robot_params.yaml')
    
#     # 声明启动参数
#     camera_source = LaunchConfiguration('camera_source')
#     robot_name = LaunchConfiguration('robot_name')
#     target_color = LaunchConfiguration('target_color')
    
#     # 参数声明
#     declare_camera_source = DeclareLaunchArgument(
#         'camera_source',
#         default_value='0',
#         description='Camera source (index or device path)'
#     )
    
#     declare_robot_name = DeclareLaunchArgument(
#         'robot_name',
#         default_value='ball_collector_robot',
#         description='Robot name for TF and namespaces'
#     )
    
#     declare_target_color = DeclareLaunchArgument(
#         'target_color',
#         default_value='red',
#         description='Target color to collect (red, blue, black, yellow)'
#     )

#     # 相机启动节点
#     camera_node = Node(
#         package='v4l2_camera',
#         executable='v4l2_camera_node',
#         name='camera_node',
#         namespace=robot_name,
#         parameters=[{
#             'video_device': camera_source,
#             'image_width': 640,
#             'image_height': 480,
#             'pixel_format': 'YUYV',
#             'camera_frame_id': 'camera_link'
#         }],
#         remappings=[
#             ('image_raw', 'camera/image_raw')
#         ]
#     )

#     # 球检测节点 - 使用基于OpenCV的检测
#     detection_node = Node(
#         package='ball_collector',
#         executable='detection_node',
#         name='detection_node',
#         namespace=robot_name,
#         parameters=[
#             {'camera_topic': ['/', robot_name, '/camera/image_raw']},
#             {'target_color': target_color},
#             {'min_ball_radius': 10},
#             {'max_ball_radius': 300},
#             {'min_circularity': 0.6},
#             {'detection_freq': 10.0},
#             {'debug_view': False}  # 禁用调试视图
#         ],
#         remappings=[
#             ('detected_ball', 'vision/detected_ball')
#         ]
#     )
    
#     # 目标区域检测节点
#     target_detection_node = Node(
#         package='ball_collector',
#         executable='target_detection_node',
#         name='target_detection_node',
#         namespace=robot_name,
#         parameters=[{
#             'camera_topic': ['/', robot_name, '/camera/image_raw'],
#             'purple_hue_low': 140,
#             'purple_hue_high': 170,
#             'purple_sat_low': 50,
#             'purple_sat_high': 255,
#             'purple_val_low': 50,
#             'purple_val_high': 255,
#             'debug_view': False  # 禁用调试视图
#         }]
#     )
    
#     # 策略节点
#     strategy_node = Node(
#         package='ball_collector',
#         executable='strategy_node',
#         name='strategy_node',
#         namespace=robot_name,
#         parameters=[
#             {'target_color': target_color},
#             {'search_angular_speed': 0.5},
#             {'approach_linear_speed': 0.2},
#             {'approach_distance': 0.3},
#             {'center_tolerance': 30},
#             {'camera_width': 640},
#             {'camera_height': 480},
#             {'ball_lost_timeout': 2.0},
#             {'max_operation_time': 300.0}
#         ],
#         remappings=[
#             ('detected_ball', 'vision/detected_ball'),
#             ('cmd_vel', 'control/cmd_vel'),
#             ('robot_state', 'status/robot_state')
#         ]
#     )
    
#     # 硬件接口节点
#     hardware_node = Node(
#         package='ball_collector',
#         executable='hardware_interface',
#         name='hardware_interface',
#         namespace=robot_name,
#         parameters=[{
#             'auto_discover': True,
#             'baud_rate': 115200,
#             'max_linear_speed': 0.3,  # 降低最大速度，提高安全性
#             'max_angular_speed': 0.8,
#             'cmd_timeout': 3.0,
#             'watchdog_timeout': 0.5
#         }],
#         remappings=[
#             ('cmd_vel', 'control/cmd_vel')
#         ]
#     )
    
#     # TF静态变换发布器 - 相机到基座的变换
#     tf_camera_node = Node(
#         package='tf2_ros',
#         executable='static_transform_publisher',
#         name='camera_link_broadcaster',
#         arguments=['0.1', '0', '0.1', '0', '0', '0', 'base_link', 'camera_link']
#     )
    
#     # 创建启动描述
#     ld = LaunchDescription()
    
#     # 添加声明参数
#     ld.add_action(declare_camera_source)
#     ld.add_action(declare_robot_name)
#     ld.add_action(declare_target_color)
    
#     # 添加节点
#     ld.add_action(camera_node)
#     ld.add_action(detection_node)
#     ld.add_action(target_detection_node)
#     ld.add_action(strategy_node)
#     ld.add_action(hardware_node)
#     ld.add_action(tf_camera_node)
    
#     return ld


from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

import os

def generate_launch_description():
    # 获取包路径
    pkg_share = get_package_share_directory('ball_collector')
    
    # 声明启动参数
    robot_name = LaunchConfiguration('robot_name')
    target_color = LaunchConfiguration('target_color')
    camera_device = LaunchConfiguration('camera_device')
    
    # 参数声明
    declare_robot_name = DeclareLaunchArgument(
        'robot_name',
        default_value='ball_collector_robot',
        description='Robot name for TF and namespaces'
    )
    
    declare_target_color = DeclareLaunchArgument(
        'target_color',
        default_value='red',
        description='Target color to collect (red, blue, black, yellow)'
    )
    
    declare_camera_device = DeclareLaunchArgument(
        'camera_device',
        default_value='/dev/video0',
        description='MIPI camera device path'
    )

    # 对于MIPI摄像头，通常使用专用的驱动程序
    # 这里假设使用标准的image_publisher节点从设备接收图像
    # 如果您有专门的MIPI摄像头ROS驱动，请相应替换此节点
    camera_node = Node(
        package='image_tools',
        executable='cam2image',
        name='camera_node',
        namespace=robot_name,
        parameters=[{
            'device': camera_device,
            'width': 640,
            'height': 480,
            'burger_mode': False,
            'frequency': 30.0
        }],
        remappings=[
            ('image', 'camera/image_raw')
        ]
    )

    # 球检测节点 - 使用基于OpenCV的检测
    detection_node = Node(
        package='ball_collector',
        executable='detection_node',
        name='detection_node',
        namespace=robot_name,
        parameters=[
            {'camera_topic': ['/', robot_name, '/camera/image_raw']},
            {'target_color': target_color},
            {'min_ball_radius': 10},
            {'max_ball_radius': 300},
            {'min_circularity': 0.6},
            {'detection_freq': 10.0},
            {'debug_view': False}  # 禁用调试视图
        ],
        remappings=[
            ('detected_ball', 'vision/detected_ball')
        ]
    )
    
    # 目标区域检测节点
    target_detection_node = Node(
        package='ball_collector',
        executable='target_detection_node',
        name='target_detection_node',
        namespace=robot_name,
        parameters=[{
            'camera_topic': ['/', robot_name, '/camera/image_raw'],
            'purple_hue_low': 140,
            'purple_hue_high': 170,
            'purple_sat_low': 50,
            'purple_sat_high': 255,
            'purple_val_low': 50,
            'purple_val_high': 255,
            'debug_view': False  # 禁用调试视图
        }]
    )
    
    # 策略节点
    strategy_node = Node(
        package='ball_collector',
        executable='strategy_node',
        name='strategy_node',
        namespace=robot_name,
        parameters=[
            {'target_color': target_color},
            {'search_angular_speed': 0.5},
            {'approach_linear_speed': 0.2},
            {'approach_distance': 0.3},
            {'center_tolerance': 30},
            {'camera_width': 640},
            {'camera_height': 480},
            {'ball_lost_timeout': 2.0},
            {'max_operation_time': 300.0}
        ],
        remappings=[
            ('detected_ball', 'vision/detected_ball'),
            ('cmd_vel', 'control/cmd_vel'),
            ('robot_state', 'status/robot_state')
        ]
    )
    
    # 硬件接口节点
    hardware_node = Node(
        package='ball_collector',
        executable='hardware_interface',
        name='hardware_interface',
        namespace=robot_name,
        parameters=[{
            'auto_discover': True,
            'baud_rate': 115200,
            'max_linear_speed': 0.3,  # 降低最大速度，提高安全性
            'max_angular_speed': 0.8,
            'cmd_timeout': 3.0,
            'watchdog_timeout': 0.5
        }],
        remappings=[
            ('cmd_vel', 'control/cmd_vel')
        ]
    )
    
    # TF静态变换发布器 - 相机到基座的变换
    tf_camera_node = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='camera_link_broadcaster',
        arguments=['0.1', '0', '0.1', '0', '0', '0', 'base_link', 'camera_link']
    )
    
    # 创建启动描述
    ld = LaunchDescription()
    
    # 添加声明参数
    ld.add_action(declare_robot_name)
    ld.add_action(declare_target_color)
    ld.add_action(declare_camera_device)
    
    # 添加节点
    ld.add_action(camera_node)
    ld.add_action(detection_node)
    ld.add_action(target_detection_node)
    ld.add_action(strategy_node)
    ld.add_action(hardware_node)
    ld.add_action(tf_camera_node)
    
    return ld